#ifndef CMVUTILS_H
#define CMVUTILS_H

#include <vector>
#include <opencv2/opencv.hpp>

namespace cmv {

/**
 * @brief 一种颜色
 */
struct Color {
	std::string name;
	int color_r;
	int color_g;
	int color_b;
	int y_min, y_max;
	int u_min, u_max;
	int v_min, v_max;
};

/**
 * @brief 三维坐标点
 */
struct Point {
	float x, y, z;
};

/**
 * @brief 图像，使用uyvy
 */
class ImageYUV422
{
public:
	ImageYUV422() : data(NULL) {}
	~ImageYUV422()
	{
		if (data) {
			delete data;
		}
	}
	unsigned width;
	unsigned height;
	unsigned char *data;
};

// From: ros cmvision
inline void rgb2yuv(int r, int g, int b, int &y, int &u, int &v)
{
	y = (306*r + 601*g + 117*b)  >> 10;
	u = ((-172*r - 340*g + 512*b) >> 10)  + 128;
	v = ((512*r - 429*g - 83*b) >> 10) + 128;
	y = y < 0 ? 0 : y;
	u = u < 0 ? 0 : u;
	v = v < 0 ? 0 : v;
	y = y > 255 ? 255 : y;
	u = u > 255 ? 255 : u;
	v = v > 255 ? 255 : v;
}

// From: ros cmvison
inline void yuv2rgb(int y, int u, int v, int &r, int &g, int &b)
{
	r = y + ((v*1436) >>10);
	g = y - ((u*352 + v*731) >> 10);
	b = y + ((u*1814) >> 10);
	r = r < 0 ? 0 : r;
	g = g < 0 ? 0 : g;
	b = b < 0 ? 0 : b;
	r = r > 255 ? 255 : r;
	g = g > 255 ? 255 : g;
	b = b > 255 ? 255 : b;
}

// From: http://www.cs.rit.edu/~ncs/color/t_convert.html
// r,g,b values are from 0 to 1
// h = [0,360], s = [0,1], v = [0,1]
//		if s == 0, then h = -1 (undefined)
inline void rgb2hsv(float r, float g, float b, float &h, float &s, float &v)
{
	float min, max, delta;

	min = r < g ? r : g;
	min = min < b ? min : b;
	max = r > g ? r : g;
	max = max > b ? max : b;
	delta = max - min;

	// v
	v = max;

	// s
	if (max) {
		s = delta / max; // (v - min) / v
	} else {
		s = 0;
		h = -1;
		return;
	}

	// h
	if (!delta) {
		h = 0;
		return;
	}
	if (r == max) {
		h = (g - b) / delta;
	} else if (g == max) {
		h = 2 + (b - r) / delta;
	} else {
		h = 4 + (r - g) / delta;
	}
	h *= 60;
	if (h < 0) {
		h += 360;
	}
}

void rgb2uyvy(unsigned char *src, unsigned char *dest, unsigned long long int NumPixels);
void uyvy2rgb(unsigned char *src, unsigned char *dest, unsigned long long int NumPixels);

bool loadImageYUV422FromIplImage(const IplImage *iplImage, ImageYUV422 &image);
bool loadImageYUV422FromMat(const cv::Mat &mat, ImageYUV422 &image);
bool loadMatFromImageYUV422(const ImageYUV422 &image, cv::Mat &mat);
std::vector<Color> loadConfig(const char *filename);
int colorIndex(const std::vector<Color> &colors, const char *name);

}
#endif // end CMVUTILS_H
